/*
 * CrateVisionNode.cpp
 *
 *  Created on: Jan 16, 2012
 *      Author: rick
 */

#include <crate_vision/main.h>
#include "crate_vision/CrateVisionNode.h"
#include "crate_vision/Detect.h"
#include "crate_vision/Initialize.h"
#include "ros/ros.h"
#include "std_msgs/String.h"

#include <string.h>

#include <cmath>
#include <iostream>
#include <stdio.h>
#include <FiducialDetector.h>
#include <pcrctransformation/pcrctransformer.hpp>
#include <pcrctransformation/point2f.hpp>

#include <huniplacer_3d/viewCheckSrv.h>

#include <xml_io/xml.h>

#include <crate_vision/TopViewMsg.h>

#include <Crate.h>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <unicap_cv_bridge.hpp>
#include <CameraCalibration/RectifyImage.h>

using namespace unicap_cv_bridge;
using namespace std;
using namespace cv;
using namespace pcrctransformation;

CrateVisionNode::CrateVisionNode(int argc, char* argv[]) : cam(atoi(argv[1]), atoi(argv[2])) {
	if (argc < 6) {
		cout << "Invalid amount of arguments gives. Requested arguments are: 1 2 <calibration.xml> <crate.xml>" << endl;
		print_devices_inventory();
	} else {
		xml_path = argv[5];
		Mat test(cam.get_img_height(), cam.get_img_width(), cam.get_img_format());
		rectifier.initRectify(argv[3], Size(cam.get_img_width(), cam.get_img_height()));

		initialized = false;


	 	getVisionData()->crateData.middle_x = 200;
		getVisionData()->crateData.middle_y = cam.get_img_height()/2 + 60;

		fidDetector = new FiducialDetector();

		init = new Initialize(&cam, &rectifier, fidDetector);
		
		//live view of the cam. Press q to continue
		cam.set_exposure(0.015);
		while(1){
			cam.get_frame(&test);
			rectifier.rectify(test, frameRectified);
			imshow("preview, press q to continue", frameRectified);
			char key = waitKey(30);
	    	if (key == 'q') {
				break;
	   	}
		}
		destroyWindow("preview, press q to continue");
		
		cout << "init exposure" << endl;
		init->exposure();

		cout << "fiducials" << endl;
/*
		while(1){
			cam.get_frame(&test);
	   	rectifier.rectify(test, frameRectified);
	   	
			char key = waitKey(0);
			if (key == 'q') {
				break;
	   	}
			init->fiducials(10, 1000);
			
			point2f fid1 = cordTransformer->to_pc(point2f(-59, -75));
	    	cout << "in: -59, -75  out: " << fid1.x << " " << fid1.y << endl;
	    	
	    	point2f fid2 = cordTransformer->to_pc(point2f(-54, 113));
	    	cout << "in: -54, 113  out: " << fid2.x << " " << fid2.y << endl;
	    	
	    	point2f fid3 = cordTransformer->to_pc(point2f(71, 110));
	    	cout << "in: 71, 110  out: " << fid3.x << " " << fid3.y << endl;
	    	
	    	draw_box(frameRectified, fid1.x - 1, fid1.y - 1, fid1.x + 1, fid1.y + 1, Scalar(0, 0, 255, 0));
	    	draw_box(frameRectified, fid2.x - 1, fid2.y - 1, fid2.x + 1, fid2.y + 1, Scalar(0, 0, 255, 0));
	    	draw_box(frameRectified, fid3.x - 1, fid3.y - 1, fid3.x + 1, fid3.y + 1, Scalar(0, 0, 255, 0));
	    	imshow("iulgluigluiguih", frameRectified);
		}		
*/
		
		if(init->fiducials(10, 1000)) {
			cout << "done!" << endl;
		} else {
			exit(1);
		}
		cout << "scale: " << getVisionData()->envData.cordTransformer->scale << endl;
		
		point2f cam_pos = getVisionData()->envData.cordTransformer->to_rc(point2f(cam.get_img_width()/2, cam.get_img_height()/2));

		getVisionData()->envData.cam_rx = cam_pos.x;
		getVisionData()->envData.cam_ry = cam_pos.y;
		getVisionData()->envData.cam_rheight = 250;
		getVisionData()->envData.cam_pheight = getVisionData()->envData.cam_rheight * getVisionData()->envData.cordTransformer->to_pc_scale;
		getVisionData()->envData.cam_px = cam.get_img_width()/2;
		getVisionData()->envData.cam_py = cam.get_img_height()/2;
		
		cout << "initialise ROS" << endl;
		ros::init(argc, argv, "vision_topview");
		ros::NodeHandle n;
		
		ros::Subscriber request = n.subscribe("topview_request", 1000, &CrateVisionNode::data_request, this);
		
		pub = n.advertise<crate_vision::TopViewMsg>("topview_info", 1000);
		
		viewCheck = n.serviceClient<huniplacer_3d::viewCheckSrv>("viewCheck");

		ros::Rate loop_rate(10);
		cout << "done" << endl;

		for(;;){
			cam.get_frame(&test);
			rectifier.rectify(test, frameRectified);

	//tekenen van fiducaials
		 	point2f fid1 = getVisionData()->envData.cordTransformer->to_pc(point2f(-65 + 1.5, -74));	
		 	point2f fid2 = getVisionData()->envData.cordTransformer->to_pc(point2f(-62.5 + 1.5, 113.5));    	
		 	point2f fid3 = getVisionData()->envData.cordTransformer->to_pc(point2f(61.5 + 1.5, 110.5));
		 	fid1 = getVisionData()->envData.cordTransformer->to_rc(fid1);
		 	fid2 = getVisionData()->envData.cordTransformer->to_rc(fid2);
		 	fid3 = getVisionData()->envData.cordTransformer->to_rc(fid3);

		 	fid1 = getVisionData()->envData.cordTransformer->to_pc(fid1);
		 	fid2 = getVisionData()->envData.cordTransformer->to_pc(fid2);
		 	fid3 = getVisionData()->envData.cordTransformer->to_pc(fid3);
		 	
		 	draw_box(frameRectified, fid1.x - 1, fid1.y - 1, fid1.x + 1, fid1.y + 1, Scalar(0, 0, 255, 0));
		 	draw_box(frameRectified, fid2.x - 1, fid2.y - 1, fid2.x + 1, fid2.y + 1, Scalar(0, 0, 255, 0));
		 	draw_box(frameRectified, fid3.x - 1, fid3.y - 1, fid3.x + 1, fid3.y + 1, Scalar(0, 0, 255, 0));

			draw_box(frameRectified, init->markers[0].x - 2, init->markers[0].y - 2, init->markers[0].x + 2, init->markers[0].y + 2, Scalar(255, 0, 0, 0));
			draw_box(frameRectified, init->markers[1].x - 2, init->markers[1].y - 2, init->markers[1].x + 2, init->markers[1].y + 2, Scalar(255, 0, 0, 0));
			draw_box(frameRectified, init->markers[2].x - 2, init->markers[2].y - 2, init->markers[2].x + 2, init->markers[2].y + 2, Scalar(255, 0, 0, 0));
		
			point2f testgrid_pix_1, testgrid_real_1;
		
		

			testgrid_real_1.x = -100;
		   testgrid_real_1.y = -120;
			
			for (int y = 0; y < 200; y+=10){
				for (int x = 0; x < 200; x+=10){
					
					testgrid_pix_1 = getVisionData()->envData.cordTransformer->to_pc(testgrid_real_1);
					if(testgrid_pix_1.x > 0 && testgrid_pix_1.x < cam.get_img_width() && testgrid_pix_1.y > 0 && testgrid_pix_1.y < cam.get_img_height()){
						cv::circle(frameRectified, cv::Point(testgrid_pix_1.x,testgrid_pix_1.y), 4, cv::Scalar(0, 0, 255), 1);
					}
					testgrid_real_1.x = -100 + x;
				}
				testgrid_real_1.y = -100 + y;
			}
				
			imshow("conversion test window, press q to continue", frameRectified);
			char key = waitKey(30);
		   if (key == 'q') {
				break;
		   }
		}
		destroyWindow("conversion test window, press q to continue");
		
		while(1) {
			ros::spin();
		}
	}
}

CrateVisionNode::~CrateVisionNode() {
	delete init;
	delete fidDetector;
	
	delete [] xml_path;
}

void CrateVisionNode::sendBox(){
	crate_vision::TopViewMsg msg;
	msg.name = crate_name;
	msg.rows = getVisionData()->crateData.row_nr;
	msg.cols = getVisionData()->crateData.col_nr;
	msg.diff_colors = getVisionData()->contentData.diff_colors;
	
	for(int i = 0; i < getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr; i++){
		msg.red.push_back(getVisionData()->crateData.crate_info_red[i]);
		msg.green.push_back(getVisionData()->crateData.crate_info_green[i]);
		msg.blue.push_back(getVisionData()->crateData.crate_info_blue[i]);
		cout << "crate_info[i]: " << getVisionData()->crateData.crate_info[i] << endl;
		msg.data.push_back(getVisionData()->crateData.crate_info[i]);
	}

	pub.publish(msg);
	ros::spinOnce();
}

void CrateVisionNode::data_request(const crate_vision::TopViewMsg::ConstPtr& msg) {
	huniplacer_3d::viewCheckSrv view;
	if (viewCheck.call(view)) {
		get_xml_data(msg->crate.name);
	
		point2f pixel_coordinates = getVisionData()->envData.cordTransformer->to_pc(point2f(msg->crate.x, msg->crate.y));
		getVisionData()->crateData.middle_x = pixel_coordinates.x;
		getVisionData()->crateData.middle_y = pixel_coordinates.y;
		getVisionData()->crateData.degree = msg->crate.angle*180/M_PI + 180;
	

		crate_name = msg->crate.name;
		getVisionData()->robotCrateData.middle_x = msg->crate.x;
		getVisionData()->robotCrateData.middle_y = msg->crate.y;
		getVisionData()->robotCrateData.degree = msg->crate.angle;

		cout << "degree: " << msg->crate.angle << endl;
		cout << "x: " << getVisionData()->crateData.middle_x << ", y: " << getVisionData()->crateData.middle_y << endl;	
	
		Detect detect(&cam, &rectifier);
	
		if (!initialized) {
			initialized = true;

			get_xml_data(msg->crate.name);
		
			getVisionData()->contentData.box = new Mat[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];

			getVisionData()->crateData.crate_info_red = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info_green = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info_blue = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];

			cout << "detections" << endl;
			init->detections();

			cout << "initialisation complete: " << getVisionData()->contentData.empty_hue_low << ", " << getVisionData()->contentData.empty_hue_high << ", " << getVisionData()->contentData.border_saturation << endl;
		} else {
			delete [] getVisionData()->contentData.box;
		
			getVisionData()->contentData.box = new Mat[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
		
			delete [] getVisionData()->crateData.crate_info_red;
			delete [] getVisionData()->crateData.crate_info_green;
			delete [] getVisionData()->crateData.crate_info_blue;
			delete [] getVisionData()->crateData.crate_info;

			getVisionData()->crateData.crate_info_red = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info_green = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info_blue = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
		
		}
	
		Mat test(cam.get_img_height(), cam.get_img_width(), cam.get_img_format());

		Mat test_final;
		cam.get_frame(&test);

		rectifier.rectify(test, test_final);

		draw_box(test_final, 
					getVisionData()->crateData.middle_x - (getVisionData()->crateData.width/2), 
					getVisionData()->crateData.middle_y - getVisionData()->crateData.height/2, 
					getVisionData()->crateData.middle_x + getVisionData()->crateData.width/2, 
					getVisionData()->crateData.middle_y + getVisionData()->crateData.height/2, 
					cv::Scalar(0, 0, 255));

		point2f testt = getVisionData()->envData.cordTransformer->to_pc(point2f(msg->crate.x, msg->crate.y));			
			
		draw_box(test_final, 
					testt.x-(getVisionData()->crateData.width/2), 
					testt.y-getVisionData()->crateData.height/2, 
					testt.x+getVisionData()->crateData.width/2, 
					testt.y+getVisionData()->crateData.height/2, 
					cv::Scalar(0, 255, 0));

		cout << "raw input: " << testt.x << ", " << testt.y << ", adjusted: " << getVisionData()->crateData.middle_x << ", " << getVisionData()->crateData.middle_y << endl;

	//	imshow("test_final", test_final); waitKey(30);

		detect.scan_crate();

		sendBox();
	} else {
		perror("Huniplacer_3d was unable to move the robot out of the way");
	}
}
/*
void CrateVisionNode::moveCallback(const vision::CrateEventMsg::ConstPtr& msg) {
	if (msg->event == CrateEvent::type_in || msg->event == CrateEvent::type_moved) {
		cout << "incx: " << msg->crate.x << ", incy: " << msg->crate.y << endl;

		point2f pixel_coordinates = getVisionData()->envData.cordTransformer->to_pc(point2f(msg->crate.x, msg->crate.y));
		getVisionData()->crateData.middle_x = pixel_coordinates.x;
		getVisionData()->crateData.middle_y = pixel_coordinates.y;
		getVisionData()->crateData.degree = msg->crate.angle*180/M_PI + 180;
		
		getVisionData()->robotCrateData.degree = msg->crate.angle;
		getVisionData()->robotCrateData.middle_x = msg->crate.x;
		getVisionData()->robotCrateData.middle_y = msg->crate.y;
		
		crate_name = msg->crate.name;
		cout << "MoveCallBack:  x: " << getVisionData()->crateData.middle_x << ", y: " << getVisionData()->crateData.middle_y << "  degree: " << getVisionData()->crateData.degree << endl;

		if (!initialized) {
			initialized = true;

			get_xml_data(msg->crate.name);
			
			getVisionData()->contentData.box = new Mat[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];

			getVisionData()->crateData.crate_info_red = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info_green = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info_blue = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];

			cout << "detections" << endl;
			init->detections();

			cout << "initialisation complete: " << getVisionData()->contentData.empty_hue_low << ", " << getVisionData()->contentData.empty_hue_high << ", " << getVisionData()->contentData.border_saturation << endl;
		} else {
			delete [] getVisionData()->contentData.box;
			
			getVisionData()->contentData.box = new Mat[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			
			delete [] getVisionData()->crateData.crate_info_red;
			delete [] getVisionData()->crateData.crate_info_green;
			delete [] getVisionData()->crateData.crate_info_blue;
			delete [] getVisionData()->crateData.crate_info;

			getVisionData()->crateData.crate_info_red = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info_green = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info_blue = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			getVisionData()->crateData.crate_info = new int[getVisionData()->crateData.col_nr * getVisionData()->crateData.row_nr];
			
			get_xml_data(msg->crate.name);
		}

		Mat test(cam.get_img_height(), cam.get_img_width(), cam.get_img_format());

		Mat test_final;
		cam.get_frame(&test);

		rectifier.rectify(test, test_final);

		draw_box(test_final, 
					getVisionData()->crateData.middle_x - (getVisionData()->crateData.width/2), 
					getVisionData()->crateData.middle_y - getVisionData()->crateData.height/2, 
					getVisionData()->crateData.middle_x + getVisionData()->crateData.width/2, 
					getVisionData()->crateData.middle_y + getVisionData()->crateData.height/2, 
					cv::Scalar(0, 0, 255));

		//pub.publish(crate_info);

		//ROS_INFO("Crate info sent!");

		//ros::spinOnce();
	}
}
*/
/*point2f CrateVisionNode::perspective_adjustment(std::string crate_name, point2f src, double angle) {
	double rc_depth, phi, rc_width, rc_height;
	point2f points[4], result;

	cout << "angle: " << angle << endl;

	utility_xml::crate_data(xml_path, crate_name, &rc_width, &rc_height, &rc_depth, NULL, NULL, NULL, NULL, NULL, NULL);
	
	for(int i = 0; i < 4; i++){
		points[i].x = rc_width/2*cos(angle + M_PI/2*i);
		points[i].y = rc_height/2*sin(angle + M_PI/2*i);
		
		cout << i << "src: " << src.x << ", " << src.y << ", x: " << points[i].x << ", " << points[i].y << endl;
		
		points[i] += src;
		
		cout << i << ", x: " << points[i].x << ", " << points[i].y << endl;
	}
	
	for(int i = 0; i < 4; i++){
		phi = atan(cam_height/(points[i].x-cam_x));
		points[i].x = rc_depth / tan(phi) + points[i].x;
		
		phi = atan(cam_height/(points[i].y-cam_y));
		points[i].y = rc_depth / tan(phi) + points[i].y;
				
		cout << "perscfgrdvf: " << i << ", x: " << points[i].x << ", " << points[i].y << endl;
	}
	
	width = hypot((points[0].x - points[2].x), (points[0].y - points[2].y));
	height = hypot((points[1].x - points[3].x), (points[1].y - points[3].y));
	
	width *= cordTransformer->to_pc_scale;
	height *= cordTransformer->to_pc_scale;
	
	box_width = width / col_nr;
	box_height = height / row_nr;
	
	result = (points[0] + points[1] + points[2] + points[3]);

	result.x/=4;
	result.y/=4; 
	
	return result;
}*/




/**
 * This function pull all data concerning crate_name from the xml
 *
 */
void CrateVisionNode::get_xml_data(String crate_name) {
	double temp_radius, temp_width, temp_height, temp_distence_to_next, temp_distance_to_side, temp_depth;

	utility_xml::crate_data(xml_path, crate_name, &temp_width, &temp_height, &temp_depth, &getVisionData()->crateData.col_nr, &getVisionData()->crateData.row_nr, &temp_distance_to_side, &temp_distence_to_next, &temp_radius, NULL);
			
	getVisionData()->crateData.offset = temp_radius * 0.5 * getVisionData()->envData.cordTransformer->to_pc_scale;
	getVisionData()->robotCrateData.offset = 0.5 * temp_radius;			
				
	getVisionData()->robotCrateData.width = temp_width;
	getVisionData()->robotCrateData.height = temp_height;
	
	getVisionData()->crateData.width = temp_width * getVisionData()->envData.cordTransformer->to_pc_scale;
	getVisionData()->crateData.height = temp_height * getVisionData()->envData.cordTransformer->to_pc_scale;
				
	getVisionData()->robotCrateData.container_width = temp_radius * 2;
	getVisionData()->robotCrateData.container_height = temp_radius * 2;
	
	getVisionData()->crateData.container_width = temp_radius*2* getVisionData()->envData.cordTransformer->to_pc_scale;
	getVisionData()->crateData.container_height = temp_radius*2* getVisionData()->envData.cordTransformer->to_pc_scale;
	
	getVisionData()->robotCrateData.centre_to_centre = temp_radius*2 + temp_distence_to_next;
	getVisionData()->crateData.centre_to_centre = (temp_radius*2 + temp_distence_to_next) * getVisionData()->envData.cordTransformer->to_pc_scale;
		
	getVisionData()->robotCrateData.distance_to_side = temp_distance_to_side;
	getVisionData()->crateData.distance_to_side = temp_distance_to_side * getVisionData()->envData.cordTransformer->to_pc_scale;
	
	getVisionData()->robotCrateData.depth = temp_depth;
	getVisionData()->crateData.depth = temp_depth * getVisionData()->envData.cordTransformer->to_pc_scale;
}

